#include <ros/ros.h>
#include <std_msgs/Float64.h>

std_msgs::Float64 g_velocity;
std_msgs::Float64 g_vel_cmd;
std_msgs::Float64 g_force;

void myCallbackVelocity(const std_msgs::Float64&message_holder)
{
    ROS_INFO("received velocity value is ：%f ",message_holder.data);
    g_velocity.data = message_holder.data;
    //通过全局变量接收消息的数据，供住函数使用
}

void myCallbackVelCmd(const std_msgs::Float64&message_holder)
{
    ROS_INFO("received velocity command value is :%f ",message_holder.data);
    g_vel_cmd.data = message_holder.data;
    //通过全局变量接收消息的数据，供住函数使用
}

int main(int argc ,char **argv)
{
    ros::init(argc , argv,"controller");
    ros::NodeHandle nh;

    ros::Subscriber my_subscriber_object1 = nh.subscribe("velocity",1,myCallbackVelocity);
    //订阅速度
    ros::Subscriber my_subscriber_object2 = nh.subscribe("vel_cmd",1,myCallbackVelCmd);
    //订阅期望的速度

    ros::Publisher my_publisher_object = nh.advertise<std_msgs::Float64>("force_cmd",1);
    //发布力的大小

    double Kv = 1.0;
    //速度控制的比例数值
    double dt_controller = 0.1;
    //这是？？？？？？？？？？？？
    double sample_rate = 1.0 / dt_controller;
    //计算更新频率是10hz
    ros::Rate naptime(sample_rate);

    g_velocity.data = 0.0;
    //速度的初始数值为0;
    g_force.data = 0.0;
    //力的初始数值为0;  但是这是一个全局变量，随着回调函数，及时更新
    g_vel_cmd.data = 0.0;;
    //初始的速度是0;
    double vel_err = 0.0;
    //速度的差值   差值 = 期望 - 实际

    while(ros::ok())
    {
        vel_err = g_vel_cmd.data - g_vel_cmd.data;//计算差值
        g_force.data = Kv * vel_err;//这里只用了 比例进行调节。不是完整的PID
        my_publisher_object.publish(g_force);//发布力的反馈数值;
        ROS_INFO ("force command = %f",g_force.data);//打印力的数值
        ros::spinOnce();//从回调函数中取得数值
        naptime.sleep();//等待按照频率的设置时间周期

    }
    return 0 ;
    //和之前的一样，
    //只要存在回调函数，
    //就不会执行这里，
    //除非ros崩溃
    
}
